﻿using System;
using System.Collections.Generic;
using System.Linq;
using System.Text;

using Microsoft.Xna.Framework;
using Radar.Navigation;

namespace Radar.PathFinding
{
    internal class PathSearcher
    {

        private OpenList _openList;
        private ClosedList _closedList;

        private NavMesh _navMesh;
        private Waypoint[] _endWaypoints;
        private bool _noPathPossible;
        private Point _endPoint;
        private AStarNode _endNode;
        private NavZone _start, _end;

        public PathSearcher(NavMesh navMesh, Point start, Point end)
        {
            _navMesh = navMesh;
            _openList = new OpenList();
            _closedList = new ClosedList();
            _noPathPossible = false;
            _endPoint = end;

            _start = navMesh.GetZoneAt(start);
            _end = navMesh.GetZoneAt(end);

            if (_start == null || _end == null)
            {
                throw new RadarException(String.Format("Cannot find a path because start ({0}) and or end ({1}) position are not on the NavMesh", start, end));
            }

            if (_start == _end)
            {
                var startNode = new AStarNode(new Waypoint(start));
                _endNode = new AStarNode(new Waypoint(end)) { Parent = startNode };
            }
            else
            {
                AddStartNode(start);
                DefineEndWaypoints(end);
            }
        }

        private void AddStartNode(Point start)
        {
            NavZone startZone = _navMesh.GetZoneAt(start);
            AStarNode startNode = new AStarNode(new Waypoint(start));
            startNode.Waypoint.Neighbours.AddRange(startZone.Waypoints);
            _openList.Add(startNode);
        }

        private void DefineEndWaypoints(Point end)
        {
            _endWaypoints = _navMesh.GetZoneAt(end).Waypoints.ToArray();
        }

        /// <summary>
        /// 
        /// </summary>
        /// <param name="searchSteps"></param>
        /// <returns></returns>
        public SearchResult SearchPath(int searchSteps)
        {
            int stepCount = 0;
            while (stepCount < searchSteps && _endNode == null && !_noPathPossible)
            {
                AStarNode current = _openList.GetAndRemoveFirst();
                if (current != null)
                {
                    ProcessNode(current);
                    _closedList.Add(current);
                }
                else
                {
                    _noPathPossible = true;
                    break;
                }
                stepCount++;
            }
            if (_endNode != null) return SearchResult.PathFound;
            if (_noPathPossible) return SearchResult.NoPathAvailable;
            return SearchResult.NotFinished;
        }

        private void ProcessNode(AStarNode node)
        {
            if (IsEnd(node.Waypoint))
            {
                _endNode = new AStarNode(new Waypoint(_endPoint)) { Parent = node };
                return;
            }
            int neighbourCount = node.Waypoint.Neighbours.Count;
            for (int i = 0; i < neighbourCount; i++)
            {
                Waypoint neighbour = node.Waypoint.Neighbours[i];
                if (!_closedList.Contains(neighbour))
                {
                    AddOrUpdateAStarNode(node, neighbour);
                }
            }
        }

        private void AddOrUpdateAStarNode(AStarNode parentNode, Waypoint waypoint)
        {
            int g = parentNode.G + GetDistance(parentNode.Waypoint.Point, waypoint.Point);
            int h = GetDistance(waypoint.Point, _endPoint);
            int f = g + h;

            var neighbourNode = _openList.GetByWaypoint(waypoint);
            if (neighbourNode == null)
            {
                // waypoint encountered for first time: add it to openList
                neighbourNode = new AStarNode(waypoint);
                neighbourNode.Parent = parentNode;
                neighbourNode.F = f;
                neighbourNode.G = g;
                neighbourNode.H = h;
                _openList.Add(neighbourNode);
            }
            else if (neighbourNode.F > f)
            {
                // waypoint already encountered, update only if this path is shorter
                neighbourNode.Parent = parentNode;
                neighbourNode.F = f;
                neighbourNode.G = g;
                neighbourNode.H = h;
                _openList.Remove(neighbourNode);
                _openList.Add(neighbourNode);
            }
        }

        private int GetDistance(Point a, Point b)
        {
            return (a.X < b.X ? b.X - a.X : a.X - b.X) + (a.Y < b.Y ? b.Y - a.Y : a.Y - b.Y);
        }

        private bool IsEnd(Waypoint waypoint)
        {
            int endCount = _endWaypoints.Length;
            for (int i = 0; i < endCount; i++)
            {
                if (_endWaypoints[i] == waypoint) return true;
            }
            return false;
        }

        public Waypoint[] GetResultPath()
        {
            if (_endNode != null)
            {
                List<Waypoint> waypoints = new List<Waypoint>();
                AStarNode node = _endNode;
                waypoints.Add(_endNode.Waypoint);
                while (node.Parent != null)
                {
                    node = node.Parent;
                    waypoints.Add(node.Waypoint);
                }
                
                int indexLast = waypoints.Count - 1;
                Waypoint[] path = new Waypoint[indexLast + 1];
                for (int i = 0; i <= indexLast; i++)
                {
                    path[i] = waypoints[indexLast - i];
                }
                return path;
            }
            else
            {
                throw new RadarException("No path found yet.");
            }
        }

    }
}
